/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/opSrc/operation_turn_around_omni.cpp
 * @Description  : 实现了OperationTurnAroundOmni类，继承自Operation类，用于全方位底盘转向操作。
 *                 代码包括初始化、启动、执行、结束全方位转向操作的各个阶段的函数实现。
 *                 通过读取传感器数据（例如相机、里程计）进行位置和角度的计算和校正，以确保 AGV 在指定位置的精确旋转。
 *                 代码还处理了数据超时、错误处理、以及对任务错误的设置。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:37:34
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#include "operation_turn_around_omni.h"
#include "tools.h"
#include "public_helper.h"
#include <eigen3/Eigen/Eigen>
#include "public_kernel.h"
#include <array>
#include <tf2/utils.h>
// #include <math.h>
using namespace Eigen;
constexpr int ODOM_DATA_TIME_OUT = 100;	  // 100MS
constexpr int CAMERA_DATA_TIME_OUT = 100; // 100MS
constexpr int PERIPH_DATA_TIME_OUT = 500; // 500MS
constexpr int DATA_LOST_LIMIT = 50;       // 50ms
constexpr float USE_MAX_SPIN_VEL_THRESHOLD = 8.0f; // 自旋时，使用8度误差作为区分高低速的门限
///////////////////////////////class OperationTurnAroundOmni implement///////////////////////////////////////////////
/**
 * @brief 构造函数
 *
 * OperationTurnAroundOmni 类是继承自 Operation 类的子类，用于实现全方位旋转操作。
 *
 * @param action 操作指令，类型为 agv_msgs::msg::Action
 */
OperationTurnAroundOmni::OperationTurnAroundOmni(const agv_msgs::msg::Action& action)
	: Operation(action)
{
}

/**
 * @brief 析构函数，用于销毁 OperationTurnAroundOmni 对象
 *
 * 在对象销毁时调用，执行一些清理工作，例如调用 LoadEnd 函数。
 */
OperationTurnAroundOmni::~OperationTurnAroundOmni()
{
	LoadEnd();
}

/**
 * @brief 初始化Omni转向操作
 *
 * 初始化Omni转向操作，包括初始化ROS相关值，并重置第一次延迟计数。
 *
 * @return 如果初始化成功，返回true；否则返回false。
 */
bool OperationTurnAroundOmni::Init()
{
	bool ret = InitRosValue();
	first_delay_count_ = 0;
	return ret;
}

/**
 * @brief 准备开始全方位旋转操作
 *
 * 在开始全方位旋转操作之前，进行必要的检查和准备工作。
 *
 * @return 如果准备成功，返回true；否则返回false。
 */
bool OperationTurnAroundOmni::ReadyStart()
{
#ifdef AGV_DEBUG_NAV
	return true;
#endif

	if (!TryCycleMarkCheck())
	{
		DEBUG_ERROR_OUT("OperationTurnAroundOmni: qrId is not Valid");
		return false;
	}

	// 做当前动作时，设置节点bit位
	FirstSetPathBitsData();
	SetPhase(PHASE_START);
	return true;
}

/**
 * @brief 开始执行动作
 *
 * 此函数用于启动OmniTurnAround操作。
 *
 * @return 返回操作是否成功启动。如果成功启动，返回true；否则返回false。
 */
bool OperationTurnAroundOmni::StartAction()
{
	std::lock_guard<std::mutex> locker(mutex_);
	finished_ = false;
#ifdef AGV_DEBUG_NAV
	return true;
#endif
	// 防止运行过程太快
	// 订阅的二维码数据还未获得
	const int delay_ms = 400, cycle = 50;
	const int max_delay_count = delay_ms / cycle; // 400ms /50ms = 8;
	if (first_delay_count_++ >= max_delay_count) {
		DEBUG_OUT("omi OperationTurnAroundOmni StartAction ok:before start delay ms:" << delay_ms << ",first_delay_count_:" << first_delay_count_);
		return true;
	}

	DEBUG_OUT("OperationTurnAroundOmni wait... time(ms):"
			  << "" << delay_ms << ",nFirstDelayCount=" << first_delay_count_);
	return false;
}

/**
 * @brief 执行函数
 *
 * 该函数用于执行Omni机器人的转向操作。
 *
 * @return 执行成功返回true，否则返回false。
 */
bool OperationTurnAroundOmni::Work()
{
	std::lock_guard<std::mutex> locker(mutex_);

#ifdef AGV_DEBUG_NAV
	return m_uTotalCount++ > 100; // 5s-500
#endif

	return CycleProc();
}

/**
 * @brief 结束动作函数
 *
 * 停止当前动作并返回 true。
 *
 * @return 如果成功停止当前动作，则返回 true；否则返回 false。
 */
bool OperationTurnAroundOmni::EndAction()
{
#ifdef AGV_DEBUG_NAV
	return true;
#endif
	Stop();
	return true;
}

/////////////////////////////////////action消息回调函数//////////////////////////////////////////

/**
 * @brief 操作转向全向轮的反馈回调函数
 *
 * 该函数用于处理操作转向全向轮时的反馈数据。
 *
 * @param feedback 操作转向全向轮的反馈数据指针
 */
void OperationTurnAroundOmni::FeedbackCb(GoalHandlePeripheral::SharedPtr,
      const std::shared_ptr<const Peripheral::Feedback> feedback)
{
	int cur_state = feedback->state;
	float cur_percent = feedback->percent_complete;
	(void)(cur_percent);
	// if (cur_percent >= 0.95f || cur_percent < 0.1f)
	{
		DEBUG_OUT("OperationTurnAroundOmni up; cur_state:" << cur_state << ",cur_percent:" << cur_percent);
	}
}

/**
 * @brief 当Omni全方位转向操作完成时，此回调函数被调用。
 *
 * 当Omni全方位转向操作完成时，此函数会根据结果更新内部状态，并输出相应的日志信息。
 *
 * @param state 目标状态，表示操作的目标状态。
 * @param result 操作结果，包含操作是否成功以及相关的消息。
 */
void OperationTurnAroundOmni::DoneCb(const GoalHandlePeripheral::WrappedResult & result)
{
	std::lock_guard<std::mutex> locker(mutex_);
    auto& result_msg = result.result;
	DEBUG_WARN_OUT("OperationTurnAroundOmni up ;Peripheral Success : " << 
			result_msg->success << ", message: " << result_msg->message);
	bool peripheral_result = result_msg->success;

	if (!peripheral_result) {
		finished_ = true;
		DEBUG_WARN_OUT("OperationTurnAroundOmni up;action finsh ok");
	} else {

		load_phase_ = LOAD_INIT;
		DEBUG_ERROR_OUT("OperationTurnAroundOmni up; action do failed:ready restart");
	}
}

/**
 * @brief OperationTurnAroundOmni::ActiveCb
 *
 * 当目标刚变为激活状态时调用的回调函数。
 *
 * 当OperationTurnAroundOmni的目标刚变为激活状态时，会调用此函数。
 * 在此函数中，仅打印一条警告信息，表示目标已激活。
 */
void OperationTurnAroundOmni::ActiveCb(GoalHandlePeripheral::SharedPtr goal_handle)
{
	DEBUG_WARN_OUT("OperationTurnAroundOmni Goal just went active");
}

/**
 * @brief 处理加载动作周期
 *
 * 根据当前加载阶段执行相应的加载动作，并在加载成功后更新加载阶段。
 *
 * @return 如果加载成功，则返回true；否则返回false。
 */
bool OperationTurnAroundOmni::HandleLoadAcitonCycle()
{
	bool ret = false;
	switch (load_phase_) {
		case LOAD_INIT:
			if (LoadInit()) {
				load_phase_ = LOAD_READY_START;
				DEBUG_OUT("omi :load run stage: enter LOAD_READY_START");
			}
			break;
		case LOAD_READY_START:
			if (LoadReadyStart()) {
				load_phase_ = LOAD_START;
				DEBUG_OUT("omi :load run stage: enter LOAD_START");
			}
			break;
		case LOAD_START:
			if (LoadStart()) {
				load_phase_ = LOAD_WORK;
				DEBUG_OUT("omi :load run stage: enter LOAD_WORK");
			}
			break;
		case LOAD_WORK:
			if (LoadWork()) {
				load_phase_ = LOAD_READY_END;
				DEBUG_OUT("omi :load run stage: enter LOAD_READY_END");
			}
			break;

		case LOAD_READY_END:
			if (LoadEnd()) {
				load_phase_ = LOAD_END;
				DEBUG_OUT("omi :load run stage: enter LOAD_END");
			}
			break;
		case LOAD_END:
			DEBUG_OUT("omi :load  run ok: LOAD_END");
			ret = true;
			break;
		case LOAD_ERROR:
			DEBUG_OUT("omi :load action do error! load run stage: LOAD_ERROR");
			break;
		default:
			break;
	}

	static int times = 0;
	if (times++ > 20) {
		times = 0;
		DEBUG_OUT("omi :cur load phase:" << load_phase_);
	}

	if (ret) {
		// SetPhase(PHASE_FINISH);
		DEBUG_OUT("omi :cur load ok ready to exit cycle:");
	}

	return ret;
}

/**
 * @brief 初始化加载方法
 *
 * 该方法用于初始化加载操作，并准备执行旋转全方位操作。
 *
 * @return 如果初始化成功，则返回true；否则返回false。
 */
bool OperationTurnAroundOmni::LoadInit()
{
#ifdef AGV_DEBUG_NAV
	return true;
#endif

	delay_count_ = 0;
	if (!OpClient::client_) {
		DEBUG_ERROR_OUT("omi up:client_  is null");
		return false;
	}

	if (!OpClient::client_->action_server_is_ready()) {
		DEBUG_ERROR_OUT("omi up:action client isServerConnected is failed");
		return false;
	}
	DEBUG_OUT("omi up:Init is ok; ready to clear(end) last action");
	LoadEnd(); // add at 2023-2-13

	return true;
}

/**
 * @brief 加载并准备启动全向运动控制
 *
 * 根据调试宏定义决定是否立即返回true，否则根据延迟计数器决定是否准备完成
 *
 * @return 准备完成返回true，否则返回false
 */
bool OperationTurnAroundOmni::LoadReadyStart()
{
#ifdef AGV_DEBUG_NAV
	return true;
#endif
	const int delay_ms = 800, cycle = 50;
	const int max_delay_count = delay_ms / cycle; // 800ms /50ms = 16;
	if (delay_count_++ >= max_delay_count) {
		DEBUG_OUT("omi up:before start delay ms:" << delay_ms << ",delay_count_:" << delay_count_);
		return true;
	}

	return false;
}

/**
 * @brief 启动负载操作
 *
 * 该函数用于启动负载操作。
 *
 * @return 如果操作成功启动，则返回true；否则返回false。
 */
bool OperationTurnAroundOmni::LoadStart()
{
	finished_ = false;
#ifdef AGV_DEBUG_NAV
	return true;
#endif
	if (!OpClient::client_ || !OpClient::client_->action_server_is_ready()) {
		DEBUG_ERROR_OUT("omi up:StartAction failed");
		return false;
	}

	try {
		auto goal_msg = Peripheral::Goal();
        goal_msg.goal_action = action_;        // 直接发送调度格式的action
        auto send_goal_options = rclcpp_action::Client<Peripheral>::SendGoalOptions();
        send_goal_options.goal_response_callback =
            std::bind(&OperationTurnAroundOmni::ActiveCb, this, std::placeholders::_1);
        send_goal_options.feedback_callback =
            std::bind(&OperationTurnAroundOmni::FeedbackCb, this, std::placeholders::_1, std::placeholders::_2);
        send_goal_options.result_callback =
            std::bind(&OperationTurnAroundOmni::DoneCb, this, std::placeholders::_1);
		
        auto goal_handle_future = OpClient::client_->async_send_goal(goal_msg, send_goal_options);
		DEBUG_OUT("omi up:Operation start ok");
	} catch (const std::exception &e) {
		DEBUG_ERROR_OUT("omi action:" << e.what());
	} catch (...) {
		DEBUG_ERROR_OUT("omi start action failed");
	}
	return true;
}

/**
 * @brief 执行负载操作
 *
 * 根据finished_标志位返回true或false。
 *
 * @return finished_为true，则返回true；否则返回false。
 */
bool OperationTurnAroundOmni::LoadWork()
{
#ifdef AGV_DEBUG_NAV
	return m_uTotalCount++ < 100; // 5s-500
#endif

	return finished_;
}

/**
 * @brief 负载操作结束
 *
 * 该函数在AGV负载操作完成后调用，用于处理一些结束时的操作。
 *
 * @return 返回true表示操作成功，否则返回false
 */
bool OperationTurnAroundOmni::LoadEnd()
{
#ifdef AGV_DEBUG_NAV
	return true;
#endif
	//.if (OpClient::client_ && OpClient::client_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
	if (OpClient::client_) {
		auto future = OpClient::client_->async_cancel_all_goals(); // important  if it is not stop tracking goals maybe is crash when callbakc is always enter (when running)
		DEBUG_OUT("omi up:#2 stopTrackingGoal");
	}

	return true;
}

////////////////////////////////////////原地矫正逻辑/////////////////////////////////////////
//
// 订阅ros相关话题
//
bool OperationTurnAroundOmni::InitRosValue()
{
	return true;
}

/**
 * @brief 设置默认参数值
 *
 * 该函数从ROS参数服务器中读取所需的参数，并设置对应的成员变量。
 *
 * 参数包括：
 * - 转向阈值（threshold_x, threshold_y, threshold_theta）
 * - 最大和最小旋转速度（max_spin_vel, min_spin_vel）
 * - 前进速度（velocity）
 * - 最大调整次数（max_adjust_times）
 * - 相对零点位置（relativeX, relativeY）
 * - 数据来源（input_source）
 * - 最大角度误差（max_angle_error）
 * - 距离探测方式（dis_detect_method）
 *
 * 在读取参数后，会对某些参数进行进一步处理，例如将阈值取绝对值，将角度阈值转换为弧度制等。
 * 最后，调用 ResetStatus 函数重置状态。
 */
void OperationTurnAroundOmni::SetDefaultValue()
{
	auto node = ROSNode();
	const std::string base_path = "turn_around.";

	// 基本阈值参数
	limit_x_ = node->get_parameter_or(base_path + "threshold_x", 0.008f);
	limit_y_ = node->get_parameter_or(base_path + "threshold_y", 0.008f);
	limit_theta_ = node->get_parameter_or(base_path + "threshold_theta", 0.6f);
	max_spin_vel_ = node->get_parameter_or(base_path + "max_spin_vel", 0.1f);
	min_spin_vel_ = node->get_parameter_or(base_path + "min_spin_vel", 0.05f);
	velocity_ = node->get_parameter_or(base_path + "vel_x", 0.01f);
	max_adjust_times_ = node->get_parameter_or(base_path + "max_adjust_times", 3);

	// 相对位置参数
	relative_zero_pos_.x_ = node->get_parameter_or(base_path + "relativeX", 0.0f);
	relative_zero_pos_.y_ = node->get_parameter_or(base_path + "relativeY", 0.0f);

	// 输入源和角度误差参数
	input_source_ = node->get_parameter_or(base_path + "input_source", static_cast<int>(INPUT_SOURCE::QR));
	max_angle_error_ = node->get_parameter_or(base_path + "max_angle_error", 30.0f);

	// 距离探测方式参数
	const std::string dis_path = base_path + "dis_detect_method.";
	dis_method_.install_x = node->get_parameter_or(dis_path + "install_x", 1.0f);
	dis_method_.channel_1 = node->get_parameter_or(dis_path + "channel1", 0);
	dis_method_.channel_2 = node->get_parameter_or(dis_path + "channel2", 1);
	dis_method_.target_dis_1 = node->get_parameter_or(dis_path + "target_dis1", 0.0f);
	dis_method_.target_dis_2 = node->get_parameter_or(dis_path + "target_dis2", 0.0f);

	dis_method_.max_dis_detect = node->get_parameter_or(dis_path + "max_dis_detect", 0.24f);
	dis_method_.ignore_y_dis_dev = node->get_parameter_or(dis_path + "ignore_y_dis_deviation", 0.02f);
	dis_method_.ignore_angle_dis_dev = node->get_parameter_or(dis_path + "ignore_angle_dis_deviation", 0.02f);

	limit_x_ = fabs(limit_x_);
	limit_y_ = fabs(limit_y_);
	limit_theta_ = TO_RADIAN(fabs(limit_theta_));
	DEBUG_OUT("omi:op:turnAround param:"
		<< "fLimitX=" << limit_x_
		<< ",fLimitY=" << limit_y_
		<< ",fLimitTheta=" << limit_theta_
		<< ",max_spin_vel_=" << max_spin_vel_
		<< ",min_spin_vel_=" << min_spin_vel_
		<< ",velocity_=" << velocity_
		<< ",max_adjust_times_=" << max_adjust_times_
		<< ",relative_zero_pos_.x=" << relative_zero_pos_.x_
		<< ",relative_zero_pos_.y=" << relative_zero_pos_.y_
		<< ",input_source_=" << input_source_
		<< ",install_x=" << dis_method_.install_x
		<< ",channel_1=" << dis_method_.channel_1
		<< ",channel_2=" << dis_method_.channel_2
		<< ",target_dis_1=" << dis_method_.target_dis_1
		<< ",target_dis_2=" << dis_method_.target_dis_2
		<< ",max_dis_detect=" << dis_method_.max_dis_detect
		<< ",ignore_y_dis_dev=" << dis_method_.ignore_y_dis_dev
		<< ",ignore_angle_dis_dev=" << dis_method_.ignore_angle_dis_dev

	);

	ResetStatus();
}

/**
 * @brief 重置状态
 *
 * 重置操作状态，包括清除相关变量和设置初始值。
 */
void OperationTurnAroundOmni::ResetStatus()
{
	// 清除变量
	current_adjust_times_ = 0;
	last_time_ = GetTickCount();
	current_line_progress_ = 0;
	cycle_ = 0.05f;
	current_camera_type_ = CAMERA_CHECK_RACK;
	// 取货变量
	load_phase_ = LOAD_INIT;
}

/**
 * @brief 设置当前阶段
 *
 * 设置Omni旋转操作的当前阶段，并清除数据丢失标志和旋转标志。
 *
 * @param phase 要设置的阶段编号
 */
void OperationTurnAroundOmni::SetPhase(int phase)
{
	phase_ = phase;
	ClearDataLostFlag();
	ClearSpinFlag();
	DEBUG_OUT("SetPhase:" << phase);
}

/**
 * @brief 清除自旋标志
 *
 * 该函数用于清除自旋标志，使对象重新进入准备旋转的初始状态。
 */
void OperationTurnAroundOmni::ClearSpinFlag()
{
	spin_phase_ = SPIN_READY_START;
	last_real_angle_ = 0.0f;
	turned_angle_ = 0.0f;
	micro_angle_diff_ = 0.0f;
}

/**
 * @brief 获取载货状态
 *
 * 根据操作数据获取当前载货状态。
 *
 * @param is_loaded 用于存储载货状态的布尔变量
 *
 * @return 成功获取状态返回true，否则返回false
 */
bool OperationTurnAroundOmni::GetLoadStatus(bool &is_loaded)
{
	const auto &op_data = GetOpData();
	auto perphStatus = op_data.periph_status_msg;
	//  判断数据的有效性，如果超时 则为无效数据
	if (IsTimeOut(perphStatus.header.stamp, ROSTime(), PERIPH_DATA_TIME_OUT))
	{
		DEBUG_ERROR_OUT("omi Turn around op: GetLoadStatus time out");
		return false;
	}
	is_loaded = perphStatus.loaded;
	return true;
}

/**
 * @brief 从地面相机数据中获取位置和角度信息
 *
 * 从操作数据中获取地面相机的位置和方向信息，并将其转换为相对位置和方向，
 * 然后将这些信息存储在提供的引用参数中。
 *
 * @param pos_x 地面相机在x轴上的相对位置（输出参数）
 * @param pos_y 地面相机在y轴上的相对位置（输出参数）
 * @param angle 地面相机的方向角度（输出参数）
 *
 * @return 如果成功获取数据，则返回true；否则返回false
 */
bool OperationTurnAroundOmni::GetGroundCameraData(float &pos_x, float &pos_y, float &angle)
{
	const auto &op_data = GetOpData();
	auto ground_camera_pose = op_data.ground_camera_pose_msg;

	Posture pose;
	bool ret = ToRelativePos(ground_camera_pose, relative_zero_pos_, pose);
	pos_x = pose.x_;
	pos_y = pose.y_;
	angle = pose.theta_;
	DEBUG_OUT("omi GetGroundCameraData:ret=" << ret << ",pos_x=" << pos_x << ",pos_y=" << pos_y << ",angle=" << angle);
	return ret;
}

/**
 * @brief 获取货架摄像头的数据
 *
 * 获取货架摄像头的位置（x, y）和角度（angle）。
 *
 * @param pos_x 货架摄像头的x坐标（输出参数）
 * @param pos_y 货架摄像头的y坐标（输出参数）
 * @param angle 货架摄像头的角度（输出参数）
 *
 * @return 如果获取数据成功，则返回true；否则返回false。
 */
bool OperationTurnAroundOmni::GetRackCameraData(float &pos_x, float &pos_y, float &angle)
{
	const auto &op_data = GetOpData();
	auto rack_camera_pose = op_data.ground_camera_pose_msg;

	Posture pose;
	bool ret = ToRelativePos(rack_camera_pose, relative_zero_pos_, pose);
	pos_x = pose.x_;
	pos_y = pose.y_;
	angle = pose.theta_;
	DEBUG_OUT("omi GetRackCameraData:ret=" << ret << ",pos_x=" << pos_x << ",pos_y=" << pos_y << ",angle=" << angle);
	return ret;
}

/**
 * @brief 获取当前位置
 *
 * 根据传感器测量得到的距离和角度，计算当前位置（x, y）和角度。
 *
 * @param pos_x 输出参数，当前位置的x坐标
 * @param pos_y 输出参数，当前位置的y坐标
 * @param angle 输出参数，当前角度
 *
 * @return 返回true表示成功获取位置，false表示失败
 */
bool OperationTurnAroundOmni::GetAnalogPos(float &pos_x, float &pos_y, float &angle)
{
	std::array<float, 2> dis = {0, 0};
	if (!GetAnalogDis(dis[0], dis[1])) {
		DEBUG_ERROR_OUT("GetAnalogPos: GetAnalogDis falied;");
		return false;
	}

	// L1-M1= Y(角度)+Y(左右)
	// L2-M2 = Y(角度)-Y(左右)
	float left_cover = dis_method_.target_dis_1 - dis[0];
	float right_cover = dis_method_.target_dis_2 - dis[1];

	float y_rotate = (left_cover + right_cover) / 2.0;	// 带正负
	float y_translate = (left_cover - right_cover) / 2.0; // 带正负

	// 计算完毕
	pos_x = 0.0f;
	pos_y = y_translate;
	angle = atan2(y_rotate, dis_method_.install_x);

	DEBUG_OUT("omi GetAnalogPos:=" << ",pos_x=" << pos_x << ",pos_y=" << pos_y << ",angle=" << angle
								   << ",dis[0]=" << dis[0] << ",dis[1]=" << dis[1]
								   << ",target_dis_1=" << dis_method_.target_dis_1
								   << ",target_dis_2=" << dis_method_.target_dis_2
								   << ",leftCover=" << left_cover
								   << ",rightCover=" << right_cover
								   << ",install_x=" << dis_method_.install_x
								   << ",Y_rotate=" << y_rotate
								   << ",Y_translate=" << y_translate

	);
	return true;
}

/**
 * @brief 判断是否可以忽略转向操作
 *
 * 此函数用于判断当前情况下是否可以忽略转向操作。
 *
 * @return 如果可以忽略转向操作则返回 true，否则返回 false。
 */
bool OperationTurnAroundOmni::CanIgnore()
{
	bool ret = false;
	std::array<float, 2> dis = {0, 0};
	if (!GetAnalogDis(dis[0], dis[1])) {
		DEBUG_ERROR_OUT("Can not Ignore :GetAnalogDis falied;");
		return ret;
	}

	// L1-M1= Y(角度)+Y(左右)
	// L2-M2 = Y(角度)-Y(左右)
	float left_cover = dis_method_.target_dis_1 - dis[0];
	float right_cover = dis_method_.target_dis_2 - dis[1];

	float y_rotate = (left_cover + right_cover) / 2.0;	// 带正负
	float y_translate = (left_cover - right_cover) / 2.0; // 带正负

	y_rotate = abs(y_rotate);
	y_translate = abs(y_translate);

	ret = y_rotate <= dis_method_.ignore_angle_dis_dev && y_translate <= dis_method_.ignore_y_dis_dev;

	DEBUG_OUT("CanIgnore is:" << (ret ? "yes" : "no")
							  << ",abs(Y_rotate) = " << y_rotate << ", abs(y_translate) = " << y_translate
							  << ",ignore_angle_dis_dev=" << dis_method_.ignore_angle_dis_dev
							  << ",ignore_y_dis_dev=" << dis_method_.ignore_y_dis_dev
							  << ",dis[0]=" << dis[0] << ",dis[1]=" << dis[1]);
	return ret;
}

/**
 * @brief 检测距离是否超出范围
 *
 * 调用此方法检测当前检测到的距离是否超过设定的最大值。
 *
 * @return 如果检测到的距离超出最大值，则返回true；否则返回false。
 */
bool OperationTurnAroundOmni::DisError()
{
	bool ret = false;
	std::array<float, 2> dis = {0, 0};

	if (GetAnalogDis(dis[0], dis[1])) {

		ret = dis[0] >= dis_method_.max_dis_detect || dis[1] >= dis_method_.max_dis_detect;
	}

	if (ret) {
		DEBUG_ERROR_OUT("error:detect distance is error; overlimit; dis[0]=" << dis[0] << ",dis[1]=" << dis[1] << ",max_dis_detect=" << dis_method_.max_dis_detect);
	}

	return ret;
}

/**
 * @brief 获取计算左前右后距离探测
 *
 * 从操作数据中获取两个距离值，并将其赋值给引用参数。
 *
 * @param dis1 第一个距离值的引用
 * @param dis2 第二个距离值的引用
 *
 * @return 如果成功获取到两个距离值，则返回true；否则返回false
 */
bool OperationTurnAroundOmni::GetAnalogDis(float &dis1, float &dis2)
{
	bool ret = false;
	const auto &op_data = GetOpData();
	auto input_analog_msg = op_data.input_analog_msg;
	auto msg = input_analog_msg;

	//  判断数据的有效性，如果超时 则为无效数据
	if (IsTimeOut(input_analog_msg.header.stamp, ROSTime(), 500)) {
		DEBUG_ERROR_OUT("GetAnalogPos time out");
		return ret;
	}
	std::array<bool, 2> find = {false, false};
	std::array<float, 2> dis = {0, 0};

	for (auto data : msg.array) {
		if (data.channel == dis_method_.channel_1) {
			dis[0] = data.value;
			find[0] = true;
		} else if (data.channel == dis_method_.channel_2) {
			dis[1] = data.value;
			find[1] = true;
		}

		if (find[0] && find[1]) {
			break;
		}
	}

	bool exist = find[0] && find[1];
	if (!exist) {
		DEBUG_ERROR_OUT("error: GetAnalogPos find channel failed;find[0]=" << find[0] << ",find[1]=" << find[1]);
		return ret;
	}

	// 如果设定的安装位置长度小于0 则错误
	if (dis_method_.install_x < 0.001) {
		DEBUG_ERROR_OUT("error: GetAnalogPos  install_x is <0;install_x=" << dis_method_.install_x);
		return ret;
	}

	dis1 = dis[0];
	dis2 = dis[1];

	return true;
}

/**
 * @brief 判断数据是否超时
 *
 * 判断当前时间与给定数据时间的差值是否超过最大允许时间。
 *
 * @param data_time 数据时间
 * @param cur_time 当前时间
 * @param max_time_ms 最大允许时间（毫秒）
 *
 * @return 如果超过最大允许时间，则返回true；否则返回false。
 *
 * @note 在AGV_DEBUG_NAV宏定义下，总是返回false。
 */
bool OperationTurnAroundOmni::IsTimeOut(const rclcpp::Time &data_time, const rclcpp::Time &cur_time, const double &max_time_ms)
{

#ifdef AGV_DEBUG_NAV
	return false;
#endif

	double time_diff_ms = (cur_time - data_time).seconds() * 1000.0;
	bool ret = time_diff_ms > max_time_ms;
	if (ret) {
		DEBUG_ERROR_OUT("omi data TimeOut:time_diff_ms=" << time_diff_ms << ",max_time_ms=" << max_time_ms);
	}
	return ret;
}

/**
 * @brief 获取里程计数据
 *
 * 从操作数据中获取里程计信息，包括线速度和角速度。
 *
 * @param vel 输出的线速度和角速度结构体
 * @param theta 输出的航向角
 *
 * @return bool 如果获取数据成功，返回true；否则返回false
 */
bool OperationTurnAroundOmni::GetOdomData(Velocity &vel, float &theta)
{
	const auto &op_data = GetOpData();
	auto odom_msg = op_data.odom_msg;

	//  判断数据的有效性，如果超时 则为无效数据
	if (IsTimeOut(odom_msg.header.stamp, ROSTime(), ODOM_DATA_TIME_OUT)) {
		DEBUG_ERROR_OUT("omi Turn around op: GetOdomData time out");
		return false;
	}

	geometry_msgs::msg::Pose data_pos = odom_msg.pose;
	geometry_msgs::msg::Twist data_speed = odom_msg.twist;
	vel.SetZero(); // 清零
	vel.vel_x_ = data_speed.linear.x;
	vel.vel_y_ = data_speed.linear.y;
	vel.angular_ = data_speed.angular.z;
	theta = 0.0f;
	try {
		theta = tf2::getYaw(data_pos.orientation);
	} catch (...) {
		DEBUG_ERROR_OUT("omi tf2::getYaw(odom.orientation) failed");
		return false;
	}

	return true;
}

/**
 * @brief 判断给定的X和Y坐标是否在限定的范围内
 *
 * 该函数用于判断给定的X和Y坐标是否满足一定的范围限制。
 *
 * @param x X坐标
 * @param y Y坐标
 *
 * @return 如果X和Y坐标都在限定的范围内，则返回true；否则返回false
 */
bool OperationTurnAroundOmni::IsXYTarget(float x, float y)
{
	bool ret = false;

	ret = fabs(x) < limit_x_ && fabs(y) < limit_y_;
	DEBUG_OUT("omi IsXYTarget;ret:" << ret << ",fX:" << x << ",limit_x_=" << limit_x_ << ",fY=" << y << ",limit_y_=" << limit_y_);

	return ret;
}

/**
 * @brief 判断给定坐标和角度是否在允许范围内
 *
 * 根据给定的x坐标、y坐标和角度，判断它们是否都在允许的范围内。
 *
 * @param x x坐标
 * @param y y坐标
 * @param angle 角度
 *
 * @return 如果x坐标、y坐标和角度都在允许的范围内，则返回true；否则返回false
 */
bool OperationTurnAroundOmni::IsXYAngleTarget(float x, float y, float angle)
{
	bool ret = false;

	ret = fabs(x) < limit_x_ && fabs(y) < limit_y_ && fabs(angle) < limit_theta_;
	DEBUG_OUT("omi IsXYAngleTarget;ret:" << ret << ",fX:" << x << ",limit_x_=" << limit_x_ << ",fY=" << y << ",limit_y_=" << limit_y_ << ",angle=" << angle
										 << ",limit_theta_=" << limit_theta_);

	return ret;
}

/**
 * @brief 检查数据丢失情况
 *
 * 检查是否有数据丢失，并在数据丢失次数超过限制时采取相应措施。
 *
 * @details
 * 如果 `data_lost_` 为 `false`，则递增 `data_lost_count_` 计数器。
 * 如果 `data_lost_count_` 计数器的值超过 `DATA_LOST_LIMIT`，则：
 * - 将 `data_lost_` 设置为 `true`，表示有数据丢失。
 * - 打印调试错误信息，指示数据丢失时的阶段（`phase_`）。
 * - 调用 `SetTaskError` 方法设置任务错误，错误类型为 `AGV_TASK_ERROR::ERROR_OP_ERROR`，错误描述为 "Lost data"，
 *   并将事件类型设置为 `agv_msgs::msg::AGVEvent::ALARM`。
 *
 * 如果 `data_lost_` 为 `true`，则不进行任何操作。
 */
void OperationTurnAroundOmni::CheckDataLost()
{
	if (!data_lost_) {
		if (data_lost_count_++ > DATA_LOST_LIMIT) {
			// 如果上读头数据丢失，启用下读头数据
			// 如果不启用下读头，则直接报失败
			data_lost_ = true;
			DEBUG_ERROR_OUT("omi Lost data, while m_nPhase=" << phase_);
			SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), "Lost data", agv_msgs::msg::AGVEvent::ALARM);
		}
	}
}

/**
 * @brief 清除数据丢失标志
 *
 * 该函数用于清除数据丢失标志，并将数据丢失计数器重置为0。
 *
 * @details
 * - 将 `data_lost_` 设置为 `false`。
 * - 将 `data_lost_count_` 重置为 0。
 * - 打印调试信息 "omi ClearDataLostFlag"。
 * - 调用 `SetTaskError` 函数，将任务错误设置为 `AGV_TASK_ERROR::ERROR_OP_ERROR`，错误信息设置为 "no error"，
 *   事件类型设置为 `agv_msgs::msg::AGVEvent::NONE`，错误发生标志设置为 `false`。
 */
void OperationTurnAroundOmni::ClearDataLostFlag()
{
	data_lost_ = false;
	data_lost_count_ = 0;
	DEBUG_OUT("omi ClearDataLostFlag");
	SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), "no error", agv_msgs::msg::AGVEvent::NONE, false);
}

/**
 * @brief 执行全向车旋转操作的周期处理函数
 *
 * 该函数是执行全向车旋转操作的周期处理函数，用于计算运行周期时间、处理异常事件，并根据当前阶段执行相应的操作。
 *
 * @return 如果操作成功返回true，否则返回false。
 */
bool OperationTurnAroundOmni::CycleProc()
{
	//
	// 计算运行的周期时间，用来速度积分来计算距离
	//
	TIME_TYPE timeNow = GetTickCount();
	cycle_ = (float)(timeNow - last_time_) / 1000.0f;
	last_time_ = timeNow;

	// 判断是否有异常事件发生，是否需要停车
	uint8_t control_mode = EventOccur(); // CancleCurTask  GetServedEventID();
	bool emg_stop = control_mode == agv_msgs::msg::AGVEvent::ESTOP;
	if (emg_stop) {
		Stop();
		DEBUG_ERROR_OUT("omi event occur,when turn around action");
		return false;
	}

	switch (phase_) {
		case PHASE_START:
		{
			// #1 开始阶段，读取原始二维码数据，计算最近qr坐标系旋转角
			int result = StartTune();
			switch (result) {
				// 运行正常进行下一步骤
				case RET_NEXT_STAGE:
					SetPhase(PHASE_CALC_SPIN_ANGLE);
					break;
				// 运行正常可以忽略矫正过程 ；直接举货
				case RET_IGNORE_ACTION:
					SetPhase(PHASE_UPDATE_POS);
					break;
				// 载货数据;是否载货;
				case RET_GET_LOAD_DATA_FAILED_ERROR:
				case RET_LOAD_STATUS_ERROR:
					SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), "omi Load status error", agv_msgs::msg::AGVEvent::ALARM);
					DEBUG_ERROR_OUT("omi Load error:int:result=" << result);
					break;
					// 二维码数据；二维码角度是否超限等错误
				case RET_GET_DATA_FAILED_ERROR:
				case RET_ANGLE_OVER_LIMIT_ERROR:
				default:
					SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), "omi camera data error or angle over limit or dis error", agv_msgs::msg::AGVEvent::ALARM);
					DEBUG_ERROR_OUT("omi camera data error or angle over limit:int:result=" << result);
					break;
			}
		}
		break;
		//  #2 计算自旋角度
		case PHASE_CALC_SPIN_ANGLE:
		{
			int result = CalcSpinAngle();
			switch (result) {
				case RET_NEXT_STAGE:
					SetPhase(PHASE_SPIN);
					DEBUG_OUT("omi PHASE_CALC_SPIN_ANGLE is ok;enter PHASE_SPIN");
					break;
				case RET_ANGLE_TARGET:
					SetPhase(PHASE_CALC_X_Y_LINE);
					DEBUG_OUT("omi PHASE_CALC_SPIN_ANGLE is ok;enter PHASE_CALC_X_Y_LINE");
					break;
				case RET_ANGLE_OVER_LIMIT_ERROR:
					SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), "omi angle over limit", agv_msgs::msg::AGVEvent::ALARM);
					DEBUG_ERROR_OUT("omi PHASE_CALC_SPIN_ANGLE: RET_ANGLE_OVER_LIMIT_ERROR");
					break;
				default:
					DEBUG_ERROR_OUT("omi CalcSpinAngle data error:result=" << result);
					break;
			}
		}
		break;
		//  #3 进行旋转动作
		case PHASE_SPIN:
			if (SpinToTarget()) {
				SetPhase(PHASE_CALC_X_Y_LINE);
				DEBUG_OUT("omi PHASE_SPIN is ok;enter PHASE_CALC_X_Y_LINE");
			}
		break;
		// #4 计算需要平移的斜线
		case PHASE_CALC_X_Y_LINE:
		{
			int result = CalcXYLine();
			switch (result) {
				case RET_NEXT_STAGE:
					SetPhase(PHASE_MOVE_LINE);
					DEBUG_OUT("omi PHASE_CALC_X_Y_LINE is ok;enter PHASE_MOVE_LINE");
					break;
				case RET_X_Y_TARGET:
					SetPhase(PHASE_CHECK_X_Y_ANGLE);
					DEBUG_OUT("omi PHASE_CALC_X_Y_LINE is ok;enter PHASE_CHECK_X_Y_ANGLE");
					break;
				default:
					DEBUG_ERROR_OUT("omi CalcSpinAngle data error:result=" << result);
					break;
			}
		}
		break;
		//  #5 平移运动一条直线
		case PHASE_MOVE_LINE:
			if (MoveLine()) {
				SetPhase(PHASE_CHECK_X_Y_ANGLE);
				DEBUG_OUT("omi PHASE_MOVE_LINE is ok;enter PHASE_CHECK_X_Y_ANGLE");
			}
	
		break;
		// #6 空闲阶段；检测是否需要再次矫正
		case PHASE_CHECK_X_Y_ANGLE:
		{
			int result = CheckXYAngle();
			switch (result) {
				case RET_X_Y_ANGLE_TARGET:
					// 三者都到位，矫正完成，同步更新当前位姿
					SetPhase(PHASE_UPDATE_POS);
					DEBUG_OUT("omi PHASE_CHECK_X_Y_ANGLE is ok;enter PHASE_UPDATE_POS");
					break;
				case RET_RESTART:
					// 再一次启动矫正
					SetPhase(PHASE_CALC_SPIN_ANGLE);
					DEBUG_OUT("omi PHASE_CHECK_X_Y_ANGLE is ok;enter PHASE_CALC_SPIN_ANGLE");
					break;
				case RET_MAX_ADJUST_ERROR:
					SetTaskError(static_cast<int>(AGV_TASK_ERROR::ERROR_OP_ERROR), "omi CheckXYAngle  max adjust error ", agv_msgs::msg::AGVEvent::ALARM);
					DEBUG_ERROR_OUT("omi CheckXYAngle max adjust error:result=" << result);
				default:
					DEBUG_ERROR_OUT("omi CheckXYAngle data error :result=" << result);
					break;
			}
		}
		break;
		// #6-1矫正完成更新位姿
		case PHASE_UPDATE_POS:
			if (UpdatePosProc()) {
				SetPhase(PHASE_LOAD);
				DEBUG_OUT("omi PHASE_UPDATE_POS is ok;enter PHASE_LOAD");
			}
		break;
		// #6-2 矫正完成等待退出循环
		case PHASE_LOAD:
			if (HandleLoadAcitonCycle()) {
				SetPhase(PHASE_FINISH);
				DEBUG_OUT("omi PHASE_LOAD is ok;enter PHASE_FINISH");
			}

		break;
		case PHASE_FINISH:
			return true;
			break;
		default:
			Stop();
			DEBUG_ERROR_OUT("omi unkown phase");
			break;
	}

	return false;
}

/**
 * @brief 开始检测
 *
 * @return 返回一个整型值，表示操作的结果。
 *         - RET_GET_LOAD_DATA_FAILED_ERROR：获取负载状态超时。
 *         - RET_LOAD_STATUS_ERROR：当前已加载，无法再次加载。
 *         - RET_ANGLE_OVER_LIMIT_ERROR：距离错误。
 *         - RET_IGNORE_ACTION：可以忽略矫正。
 *         - RET_NEXT_STAGE：进入下一阶段。
 */
int OperationTurnAroundOmni::StartTune()
{
	SetDefaultValue();

	bool is_load = false;
	if (!GetLoadStatus(is_load)) {
		DEBUG_ERROR_OUT("omi errro:Start tune around action:GetLoadStatus time out");
		return RET_GET_LOAD_DATA_FAILED_ERROR;
	}
	if (is_load) {
		DEBUG_ERROR_OUT("omi error: Start tune around curLoad is already load=" << is_load);
		return RET_LOAD_STATUS_ERROR;
	}

	int ret = RET_NEXT_STAGE;
	if (input_source_ == INPUT_SOURCE::QR) {
		ret = SetRelativePos();
	} else if (input_source_ == INPUT_SOURCE::ANALOG) {
		if (DisError()) {
			// 报错:距离错误
			ret = RET_ANGLE_OVER_LIMIT_ERROR;
		} else if (CanIgnore()) {
			// 是否可以忽略矫正
			ret = RET_IGNORE_ACTION;
		}
	}
	// 其他方式直接进入下一阶段

	DEBUG_OUT("omi Start tune around action:StartTune once");
	return ret;
}

/**
 * @brief 计算旋转角度
 *
 * 计算旋转角度以调整货架的姿态，确保货架正确对准。
 *
 * @return 返回值表示函数执行的结果：
 *         - RET_ANGLE_TARGET：旋转角度达到目标值
 *         - RET_ANGLE_OVER_LIMIT_ERROR：旋转角度超出限制
 *         - RET_NEXT_STAGE：需要进入下一个阶段
 *         - RET_GET_DATA_FAILED_ERROR：获取数据失败
 */
int OperationTurnAroundOmni::CalcSpinAngle()
{
	// 复位上一个阶段的状态，为再次调整做准备
	ResetStatus();

	float x, y, angle;
	// 从摄像机读取到货架的姿态误差
	bool read = ReadPos(x, y, angle);
	if (read) {
		ClearDataLostFlag();
		// float fAbsAngle = relative_zero_pos_.theta_;
		float diff = AngleDiff(0.0f, angle);
		if (diff <= limit_theta_) {
			Stop();
			DEBUG_OUT("omi CalcTargetAngle is ok;"
					  << ",current angle=" << angle << ",diff=" << diff << ",fLimitTheta=" << limit_theta_);
			return RET_ANGLE_TARGET;
		}
		angle = Angle::NormAngle2(angle); //[-PI,PI)

		if (fabs(angle) > TO_RADIAN(max_angle_error_)) {
			DEBUG_ERROR_OUT("omi CalcSpinAngle too big:angle=" << angle);
			return RET_ANGLE_OVER_LIMIT_ERROR;
		}

		move_data_.spin_target_angle = Angle::NormAngle2(-angle);
		DEBUG_OUT("omi CalcTargetAngle:angle=" << angle
												<< ",fSpinTargetAngle=" << move_data_.spin_target_angle);
		return RET_NEXT_STAGE;
	} else {
		Stop();
		CheckDataLost();
	}

	return RET_GET_DATA_FAILED_ERROR;
}

/**
 * @brief 直线移动消除x y方向偏差
 *
 * 直线移动函数，调用 StraightMove 函数，将 move_data_ 数据传递给该函数进行移动。
 *
 * @return 如果移动成功，则返回 true；否则返回 false。
 */
bool OperationTurnAroundOmni::MoveLine()
{
	return StraightMove(move_data_);
}

/**
 * @brief 旋转到目标位置
 *
 * 将机器人旋转到指定的目标角度。
 *
 * @return 如果成功旋转到目标位置，则返回 true；否则返回 false。
 */
bool OperationTurnAroundOmni::SpinToTarget()
{
	bool ret = SpinMove(move_data_.spin_target_angle);
	float x, y, angle;
	bool read = ReadPos(x, y, angle);
	if (read) {
		if (AngleDiff(0, angle) < limit_theta_) {
			Stop();
			ret = true;
			DEBUG_OUT("omi SpinToTarget:target ok angle=" << angle);
		}
	}
	return ret;
}

/**
 * @brief 用于检查货架的姿态误差
 *
 * 货架调整过程中，不断检查货架的姿态误差，并根据误差的大小决定是否需要重新调整。通过这种方式，可以确保货架的姿态误差在可接受范围内。
 *
 * @return 操作的结果 RET_X_Y_ANGLE_TARGET：货架的姿态误差已小于门限值，调整成功。
 *					RET_RESTART：需要重新调整货架的姿态。
 *					RET_MAX_ADJUST_ERROR：调整次数已达到最大值，调整失败。
 *					RET_GET_DATA_FAILED_ERROR：未能读取到货架的位置信息，调整失败。
 */
int OperationTurnAroundOmni::CheckXYAngle()
{
	float x, y, angle;
	// 从摄像机读取到货架的姿态误差
	bool read = ReadPos(x, y, angle);
	if (read) {
		ClearDataLostFlag();
		// 如误差已小于门限，则位置核查通过
		if (IsXYAngleTarget(x, y, angle)) {
			Stop();
			return RET_X_Y_ANGLE_TARGET;
		} else {
			if (current_adjust_times_ < max_adjust_times_) {
				current_adjust_times_++;
				DEBUG_OUT("omi ready adjust agin: StartPreTune"
						  << ",nCurAdjustTimes:" << current_adjust_times_ << ",max_adjust_times_=" << max_adjust_times_);
				return RET_RESTART;
			} else {
				Stop();
				DEBUG_ERROR_OUT("omi OperationTurnAroundOmni: failed: nCurAdjustTimes:" << current_adjust_times_ << ",max_adjust_times_=" << max_adjust_times_);
				return RET_MAX_ADJUST_ERROR;
			}
		}
	} else {// 如未能从摄像机得到货架位置信息
		Stop();
		CheckDataLost();
	}

	DEBUG_OUT("omi OperationTurnAroundOmni"
			  << ",nCurAdjustTimes:" << current_adjust_times_);
	return RET_GET_DATA_FAILED_ERROR;
}

/**
 * @brief 更新位置信息
 *
 * @return 成功返回 true；否则返回 false。
 */
bool OperationTurnAroundOmni::UpdatePosProc()
{
	bool ret = false;
	SetPausePreCompile(false);
	Stop();
	DEBUG_OUT("omi OperationTurnAroundOmni: finish ok,ready to exit current cycle;ready to ForceUpdataCurPos");
	ret = ForceUpdataCurPos();
	return ret;
}

/**
 * @brief 清除角度误差。
 * angle为弧度制的相对角，可取正、负，旋转方向由angle方向确定
 * 
 * @return 成功返回 true；否则返回 false。
 */
bool OperationTurnAroundOmni::SpinMove(float angle)
{
	// 如目标转角已达到
	float diff_angle = angle > 0.0f ? (angle - turned_angle_) : (turned_angle_ - angle);
	if (diff_angle < limit_theta_ / 2) {
		Stop();
		DEBUG_OUT("omi SpinMove OK;fHaveTurnedAngle=" << turned_angle_ << ",target angle="
													  << angle << ",fLimitTheta/2=" << limit_theta_ / 2);
		return true;
	}

	float real_angle;
	// 此处使用里程计角度
	if (!GetRelAngle(real_angle)) {
		Stop();
		DEBUG_ERROR_OUT("omi read odom angle is failed");
		return false;
	}
	real_angle = Angle::NormAngle(real_angle); // [0, 2*PI).

	// SPIN_READY_START 阶段:开始进入旋转，初始化变量
	if (spin_phase_ == SPIN_READY_START) {
		last_real_angle_ = real_angle;
		turned_angle_ = 0.0f;
		micro_angle_diff_ = 0.0f;
		spin_phase_ = SPIN_START_OK;
		DEBUG_OUT("omi enter SPIN_READY_START once");
		return false;
	}

	// SPIN_START_OK 阶段:开始旋转
	// 累加微小的角度(积分角度)
	micro_angle_diff_ = real_angle - last_real_angle_;
	micro_angle_diff_ = Angle::NormAngle2(micro_angle_diff_); // [-PI, PI).
	if (fabs(micro_angle_diff_) < TO_RADIAN(90)) {			   // 如果4.5*20周期 = 90degree/每秒 则数据跳变
		turned_angle_ += micro_angle_diff_;
		last_real_angle_ = real_angle; // 将当前角度设置为角度记录
		DEBUG_OUT("omi SpinMove : Enter Turn");
	} else {
		spin_phase_ = SPIN_READY_START;
		DEBUG_ERROR_OUT("omi SpinMove : real angle Jumped;angle is too big");
		Stop();
		return false;
	}

	DEBUG_OUT("omi SpinMove : fHaveTurnedAngle=" << turned_angle_ << ",fMicroAngleDiff=" << micro_angle_diff_
												 << ",angle=" << angle
												 << ",diff_angle=" << diff_angle);

	// 如果角度大于10度,分阶梯自旋速速
	bool use_max_vel = fabs(angle) > TO_RADIAN(USE_MAX_SPIN_VEL_THRESHOLD) && fabs(diff_angle) > TO_RADIAN(USE_MAX_SPIN_VEL_THRESHOLD);
	float spin_max_vel = fabs(max_spin_vel_);
	float spin_min_vel = fabs(min_spin_vel_);

	float spin_vel = use_max_vel ? spin_max_vel : spin_min_vel;
	spin_vel = __max_(spin_vel, 0.01f);
	float ang_vel = (angle > 0.0f) ? spin_vel : -spin_vel;
	// 输出
	SetAutoModeNavVel(0, 0, ang_vel);

	return false;
}

/**
 * @brief 计算目标的X和Y坐标
 *  读取摄像机到货架的姿态误差，并判断目标是否到达。
 *  如果目标已到达，则停止操作并返回目标状态；如果未到达，则更新目标距离并返回下一阶段。
 *  如果读取数据失败，则停止操作并检查数据丢失情况。
 * 
 * @return 成功返回 true；否则返回 false。
 */
int OperationTurnAroundOmni::CalcXYLine()
{
	float x, y, angle;
	// 从摄像机读取到货架的姿态误差
	bool read = ReadPos(x, y, angle);
	if (read) {
		ClearDataLostFlag();
		if (IsXYTarget(x, y)) {
			Stop();
			DEBUG_OUT("omi CalcXYLine x y is target is ok ;"
					  << ",current fX=" << x << ",fY=" << y << ",angle=" << angle);
			return RET_X_Y_TARGET;
		}
		move_data_.x_target_dis = -x;
		move_data_.y_target_dis = -y;
		DEBUG_OUT("omi CalcXYLine:fXTargetDis=" << move_data_.x_target_dis
												<< ",fYTargetDis=" << move_data_.y_target_dis);
		return RET_NEXT_STAGE;
	} else 	{
		Stop();
		CheckDataLost();
	}
	return RET_GET_DATA_FAILED_ERROR;
}

/**
 * @brief 执行指定的直线动作。
 *  该函数用于控制机器人在直线路径上移动。
 *  根据目标的x和y方向距离计算出所需的转向角度和速度。
 *  根据实际或理论速度更新当前进度，并判断是否已达到设定的目标距离。
 * 
 * @return 如果目标距离已达到，true；否则返回 false。
 */
bool OperationTurnAroundOmni::StraightMove(MOVE_DATA move_data)
{
	bool ret = false;
	// #1 消除fx,fy方向的偏差
	float x = move_data.x_target_dis;
	float y = move_data.y_target_dis;
	float steer_angle = atan2(y, x);
	Velocity control_vel;
	control_vel.vel_x_ = SIGN(x) * fabs(cos(steer_angle)) * velocity_;
	control_vel.vel_y_ = SIGN(y) * fabs(sin(steer_angle)) * velocity_;
	control_vel.angular_ = 0.0f;

	// 使用距离长的作为判断
	bool use_x = fabs(x) > fabs(y);

	float dist = use_x ? x : y;
	float axis_vel = use_x ? control_vel.vel_x_ : control_vel.vel_y_;

	axis_vel = Limit(axis_vel, 0.2f);

	// 统计进度
	Velocity vel;
	if (GetRealVel(vel)) { // 使用真实的轴速度
		float realAxisVel = use_x ? vel.vel_x_ : vel.vel_y_;
		current_line_progress_ += realAxisVel * cycle_;
	} else {
		// 使用理论的轴速度
		current_line_progress_ += axis_vel * cycle_;
		DEBUG_ERROR_OUT("omi get odom speed failed;so force use theory speed");
	}

	DEBUG_OUT("omi In straight move, "
			  << ",fx=" << x
			  << ",fy=" << y
			  << ",bUseX=" << use_x
			  << ",CurLineProg= " << current_line_progress_
			  << ", fDist=" << dist
			  << ", cycle = " << cycle_
			  << ",fAxisVel=" << axis_vel);

	// 如果进度已满，则结束此动作
	if (fabs(current_line_progress_) >= fabs(dist)) {
		control_vel.SetZero();
		ret = true;
		DEBUG_OUT("omi Line full dist covered: m_fCurLineProg = " << current_line_progress_);
	}

	// 输出速度
	SetAutoModeNavVel(control_vel.vel_x_, control_vel.vel_y_, 0);
	return ret;
}

////////////////////////////辅助方法/////////////////////
/**
 * @brief 车体停止
 *
 */
void OperationTurnAroundOmni::Stop()
{
	DEBUG_OUT("omi OperationTurnAroundOmni::Stop");
	// 停车
	SetAutoModeNavVel(0, 0, 0);
}

/**
 * @brief 读取位置数据
 *
 * @return 如果获取成功，返回 true；否则返回 false。
 */
bool OperationTurnAroundOmni::ReadPos(float &pos_x, float &pos_y, float &angle)
{
	bool ret = false;
	switch (input_source_) 	{
		case INPUT_SOURCE::QR:
			ret = ReadCameraPos(current_camera_type_, pos_x, pos_y, angle);
			break;
		case INPUT_SOURCE::ANALOG:
			ret = GetAnalogPos(pos_x, pos_y, angle);
			break;
		default:
			DEBUG_ERROR_OUT("error:unkown input source=" << input_source_);
			break;
	}

	return ret;
}

/**
 * @brief 读取相机数据
 *
 * @return 如果获取成功，返回 true；否则返回 false。
 */
bool OperationTurnAroundOmni::ReadCameraPos(int type, float &pos_x, float &pos_y, float &angle)
{
	bool ret = false;
	switch (type) 	{
		case CAMERA_CHECK_GROUND:
			ret = GetGroundCameraData(pos_x, pos_y, angle);
			break;
		case CAMERA_CHECK_RACK:
			ret = GetRackCameraData(pos_x, pos_y, angle);
			break;

		default:
			DEBUG_ERROR_OUT("omi ReadCameraPos failed:unkown type:" << type);
			break;
	}

	return ret;
}

/**
 * @brief 使用里程计当前角度或IMU相对角度
 *
 * @return 如果获取成功，返回 true；否则返回 false。
 */
bool OperationTurnAroundOmni::GetRelAngle(float &rel_angle)
{
	Velocity vel;
	return GetOdomData(vel, rel_angle);;
}

/**
 * @brief  使用里程计获取实际速度反馈值
 *
 * @return 如果获取成功，返回 true；否则返回 false。
 */
bool OperationTurnAroundOmni::GetRealVel(Velocity &vel)
{
	float angle;
	return GetOdomData(vel, angle);
}


/**
 * @brief  转换任意位置为相对零点
 *  T_BaseToCamera: camera 相对 base
 *  该函数将相机数据位置转换为相对位置。通过原始数据计算出车体中心的坐标系变换矩阵，
 *  然后计算相对坐标系的变换，最后输出相对位置的坐标和角度。
 * 
 * @return 如果获取成功，返回 true；否则返回 false。
 */
bool OperationTurnAroundOmni::ToRelativePos(const geometry_msgs::msg::PoseStamped &camera_data_pos, 
											const Posture &input_relative_pos, Posture &output_data_pos)

{
	float pos_x = input_relative_pos.x_, pos_y = input_relative_pos.y_, angle = input_relative_pos.theta_;

	DEBUG_OUT("omi inputRelativePos:pos_x=" << pos_x << ",pos_y=" << pos_y << ",fPosAngle=" << angle);
	Posture raw_pose;
	if (!RawDataToPos(camera_data_pos, raw_pose)) {
		DEBUG_ERROR_OUT("ToRelativePos RawDataToPos failed");
		return false;
	}

	DEBUG_OUT("ToRelativePos RawDataToPos ok:x=" << raw_pose.x_ << ",y=" << raw_pose.y_ << ",theta=" << raw_pose.theta_);

	Matrix3f T_QrToBase, T_BaseToQr;
	Matrix3f T_QrToRelQr, T_BaseToRelQr, T_RelQrToBase;

	// 原始数据矩阵
	T_QrToBase << cosf(raw_pose.theta_), -sinf(raw_pose.theta_), raw_pose.x_,
		sinf(raw_pose.theta_), cosf(raw_pose.theta_), raw_pose.y_,
		0, 0, 1;

	T_BaseToQr = T_QrToBase.inverse();

	// 需要旋转Qr坐标系变换量:pos_x =pos_y =0;暂时只需要旋转方向
	T_QrToRelQr << cosf(angle), -sinf(angle), pos_x,
		sinf(angle), cosf(angle), pos_y,
		0, 0, 1;

	// Qr相对于车体中心
	T_BaseToRelQr = T_BaseToQr * T_QrToRelQr;

	// 新Qr到Base变换
	T_RelQrToBase = T_BaseToRelQr.inverse();

	Matrix3d RelQrToBase_RM;
	RelQrToBase_RM << T_RelQrToBase(0, 0), T_RelQrToBase(0, 1), 0,
		T_RelQrToBase(1, 0), T_RelQrToBase(1, 1), 0,
		0, 0, 1;

	::Quaterniond QuaterRelQrToBase = ::Quaterniond(RelQrToBase_RM);
	QuaterRelQrToBase.normalize();

	float out_x = T_RelQrToBase(0, 2);
	float out_y = T_RelQrToBase(1, 2);

	geometry_msgs::msg::Pose pose;
	pose.orientation.w = QuaterRelQrToBase.w();
	pose.orientation.x = 0;
	pose.orientation.y = 0;
	pose.orientation.z = QuaterRelQrToBase.z();
	float out_angle = 0.0f;
	try
	{
		out_angle = tf2::getYaw(pose.orientation);
	} catch (...) {
		DEBUG_ERROR_OUT("omi tf2::getYaw(QtoRelBaseR.orientation) failed");
		return false;
	}
	output_data_pos.x_ = out_x;
	output_data_pos.y_ = out_y;
	output_data_pos.theta_ = out_angle;
	DEBUG_OUT("omi ToRelativePos: fOutX=" << out_x << ",fOutY=" << out_y << ",fOutAngle=" << out_angle);

	return true;
}

/**
 * @brief  转换相机的数据为位置信息
 *  该函数将来自相机的数据转换为位置信息，检查数据的有效性并提取位置和朝向信息。
 * 
 * @return 如果获取成功，返回 true；否则返回 false。
 */
bool OperationTurnAroundOmni::RawDataToPos(const geometry_msgs::msg::PoseStamped &camera_data_pos, Posture &output_data_pos)
{
	//  判断数据的有效性，如果超时 则为无效数据
	if (IsTimeOut(camera_data_pos.header.stamp, ROSTime(), CAMERA_DATA_TIME_OUT)) {
		DEBUG_ERROR_OUT("omi Turn around op: GetCameraData time out");
		return false;
	}

	geometry_msgs::msg::Pose data_pos = camera_data_pos.pose;
	output_data_pos.x_ = data_pos.position.x;
	output_data_pos.y_ = data_pos.position.y;
	float tag = data_pos.position.z;
	float data_angle = 0.0f;
	try {
		data_angle = tf2::getYaw(data_pos.orientation);
	} catch (...) {
		DEBUG_ERROR_OUT("omi tf2::getYaw(data_pos.orientation) failed");
		return false;
	}
	output_data_pos.theta_ = data_angle;
	DEBUG_OUT("omi raw data:fTag=" << tag << ",fX=" << output_data_pos.x_ << ",fY=" << output_data_pos.y_ << ",fTheta=" << output_data_pos.theta_);
	return true;
}

/**
 * @brief  规范角度值
 *  将输入的角度 theta 规范化到 [0, 2PI] 范围内，并计算该角度是 π/2 的多少倍（四舍五入取整）。
 * 
 * @return 90度的倍数。
 */
int OperationTurnAroundOmni::MatchToMultipleOfPi_2(float theta)
{
	//[0,2PI]
	theta = Angle::NormAngle(theta);
	// 计算是pi/2的几倍（取整数部分）四舍五入
	return static_cast<int>(std::round(theta / PI_2));
}

/**
 * @brief 计算给定角度theta与最接近的PI/2的余角度。
 *  将theta规范化到[0, 2pi]范围内，找到与theta最接近的PI/2的倍数n
 *  
 * @return 返回theta与n * PI/2的绝对余角。
 */
float OperationTurnAroundOmni::RemainderDivPi_2(float theta)
{
	//[0,2pi]
	theta = Angle::NormAngle(theta);

	// 几个PI/2 四舍五入
	int n = MatchToMultipleOfPi_2(theta);

	// 取靠近pi/2的最小 余角度(可正负，此处只取绝对值)
	float remainder = theta - n * PI_2;
	return fabs(remainder);
}

/**
 * @brief 获取当前相机的原始数据。
 * 获取当前相机的原始数据，根据当前相机类型返回相应的位姿信息。
 *  
 * @return 如果相机类型未知，则输出错误信息。
 */
geometry_msgs::msg::PoseStamped OperationTurnAroundOmni::GetCurCameraRawData()
{
	const auto &op_data = GetOpData();
	auto ground_camera_pose = op_data.ground_camera_pose_msg;
	auto rack_camera_pose = op_data.rack_camera_pose_msg;

	geometry_msgs::msg::PoseStamped raw_pose;
	switch (current_camera_type_) {
		case CAMERA_CHECK_GROUND:
			raw_pose = ground_camera_pose;
			break;
		case CAMERA_CHECK_RACK:
			raw_pose = rack_camera_pose;
			break;
		default:
			DEBUG_ERROR_OUT("omi GetCurCameraRawData failed:unkown type:" << current_camera_type_);
			break;
	}
	return raw_pose;
}

/**
 * @brief 设置相对位置。
 * 获取当前相机数据并将其转换为位置格式。
 * 计算当前角度与π/2的倍数匹配，并检查剩余角度是否在允许范围内。
 *  
 * @return 返回成功状态，如果转换失败，返回错误。
 */
int OperationTurnAroundOmni::SetRelativePos()
{
	Posture raw_pose;
	// 获取当前使用的相机数据
	auto camera_data = GetCurCameraRawData();
	// 原始数据转换到x y t格式
	if (!RawDataToPos(camera_data, raw_pose)) {
		DEBUG_ERROR_OUT("error:SetRelativePos RawDataToPos time out ");
		return RET_GET_DATA_FAILED_ERROR;
	}

	float angle = raw_pose.theta_;
	// 匹配pi/2 的整数倍数-四舍五入
	int k = MatchToMultipleOfPi_2(angle);

	// pi/2 余数角度
	float remain_theta = RemainderDivPi_2(angle);

	// 设置二维码坐标系需要旋转的角度方向
	relative_zero_pos_.theta_ = k * PI_2;

	DEBUG_OUT("SetRelativePos; k=" << k << ",remainTheta=" << remain_theta << "relativeZeroPos.fThita=" << relative_zero_pos_.theta_);

	if (fabs(remain_theta) > TO_RADIAN(max_angle_error_)) {
		DEBUG_ERROR_OUT("error:SetRelativePos falied remainAngle > 30 degree;remainTheta = " << remain_theta);
		return RET_ANGLE_OVER_LIMIT_ERROR;
	}
	DEBUG_OUT("SetRelativePos ok;relativeAngle=" << relative_zero_pos_.theta_);
	return RET_NEXT_STAGE;
}

// void OperationTurnAroundOmni::DebugInit()
// {
// 	StartTune();
// 	Init();
// }
// void OperationTurnAroundOmni::DebugCycle()
// {
// 	float fX, fY, angle;
// 	// 从摄像机读取到货架的姿态误差
// 	bool ret = ReadCameraPos(current_camera_type_, fX, fY, angle);
// 	DEBUG_WARN_OUT("DebugCycle:x=" << fX << ",y=" << fY << ",angle=" << angle);
// }
